function [r, R] = adaptmeasnoiseupd_sagehusa(updMean, z, x, P, H, cycCnt, b) %#codegen
%ADAPTMEASNOISEUPD_SAGEHUSA Sage-Husa自适应量测噪声估计 % TODO: 尝试文献1中另一种避免负定的方法
%
% Input Arguments:
% # updMean: 更新标志，true表示更新噪声均值和方差，false表示只更新方差（用于噪声均值为零）
% # z: n*1列向量，量测量
% # x: m*1列向量，当前周期滤波器外推后的状态向量
% # P: m*m矩阵，卡尔曼滤波器外推计算的当前周期的状态误差协方差矩阵
% # H: n*m矩阵，量测矩阵
% # cycCnt: 标量，更新周期计数，从1开始
% # b: 标量，遗忘因子，（0<b<1）通常取0.95~0.99，若b=NaN,则采用1/cycCnt计算遗忘因子
%
% Output Arguments:
% # r: n*1列向量，当前周期观测噪声序列均值
% # R: n*n矩阵，当前周期观测噪声序列协方差矩阵
%
% References:
% # 《应用导航算法工程基础》“量测噪声估计”

persistent p_adaptmeasnoiseupd_sagehusa_r p_adaptmeasnoiseupd_sagehusa_R

if isempty(p_adaptmeasnoiseupd_sagehusa_r)
    p_adaptmeasnoiseupd_sagehusa_r = coder.nullcopy(zeros(size(z)));
end
if isempty(p_adaptmeasnoiseupd_sagehusa_R)
    p_adaptmeasnoiseupd_sagehusa_R = coder.nullcopy(zeros(size(z, 1)));
end
% 计算比例因子
if isnan(b)
    dn = 1/cycCnt;
else
    dn = (1-b)/(1-b^cycCnt);
end
% 更新量测噪声均值
if updMean
    p_adaptmeasnoiseupd_sagehusa_r = (1-dn)*p_adaptmeasnoiseupd_sagehusa_r + dn*(z-H*x);
else
    p_adaptmeasnoiseupd_sagehusa_r = zeros(size(z));
end
% 更新量测噪声方差
zTilde = z - H*x - p_adaptmeasnoiseupd_sagehusa_r;
RTmp = dn*(zTilde*zTilde' - H*P*H');
RTmp = diag(abs(diag(RTmp))); % 避免负定的方法选为: 2绝对值法
p_adaptmeasnoiseupd_sagehusa_R = (1-dn)*p_adaptmeasnoiseupd_sagehusa_R + RTmp;
% 输出
r = p_adaptmeasnoiseupd_sagehusa_r;
R = p_adaptmeasnoiseupd_sagehusa_R;
end